package com.sparrow.common.ai.findpath.impl;

import com.sparrow.common.ai.findpath.PathNode;
import java.util.*;

/**
 * JPS 跳点搜索 实际上就是A Star的改进版本。
 * 对A Star open List 进行裁剪。跳点才会加入openList
 * <p>
 * 在网格上，JPS会通过跳跃到特定点（称为跳点），而不是逐个节点进行搜索。跳点是那些可以改变方向或具有特殊意义的节点
 */
public class JPS {
    /**
     * 将要被估算的节点集合，初始只包含起始点
     */
    private PriorityQueue<PathNode> openList = new PriorityQueue<>();
    /**
     * 已经被估算的节点集合
     */
    private Set<PathNode> closeSet = new HashSet<>();

    private LinkedList<PathNode> neighbors = new LinkedList<>();

    private int[][] grid;

    public List<PathNode> findPath(int[][] mapData, int[] start, int[] goal) {
        this.grid = mapData;

        PathNode goalNode = new PathNode(goal[0], goal[1]);
        PathNode startNode = new PathNode(start[0], start[1]);
        startNode.setG(0);
        startNode.setF(startNode.getG() + h(startNode, goalNode));
        openList.add(startNode);
        while (!openList.isEmpty()) {
            PathNode current = openList.poll();
            if (current.equals(goalNode)) {
                return reconstructPath(current);
            }
            closeSet.add(current);

            findNeighbors(current);
            for (PathNode n : neighbors) {
                PathNode jumpNode = jump(n, current, goalNode);
                if (jumpNode != null && !closeSet.contains(jumpNode)) {
                    int newG = current.g + distance(current, jumpNode);
                    if (!openList.contains(jumpNode) || newG < jumpNode.g) {
                        jumpNode.g = newG;
                        jumpNode.h = h(jumpNode, goalNode);
                        jumpNode.f = jumpNode.g + jumpNode.h;
                        jumpNode.parent = current;
                        if (!openList.contains(jumpNode)) {
                            openList.add(jumpNode);
                        }
                    }
                }
            }
        }
        return Collections.emptyList();
    }

    private int distance(PathNode a, PathNode b) {
        return (int) (Math.sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y)) * 10); // Euclidean distance
    }

    private PathNode jump(PathNode n, PathNode cur, PathNode end) {
        // 一定可达
        if (n == null) {
            return null;
        }
        if (n.equals(end)) { //目标也是跳点
            return n;
        }
        //算出方向
        int dx = getDx(n, cur);
        int dy = getDy(n, cur);

        // 斜线方向前进
        if (dx != 0 && dy != 0) {
            // dx = 1 && dy = 1时，(左下可走 && 左边障碍物) || (右斜上可行走 && 上边障碍物) ，，，此时n有强迫邻居左边 上边
            if ((isWalkable(n.x - dx, n.y + dy) && !isWalkable(n.x - dx, n.y))
                    || (isWalkable(n.x + dx, n.y - dy) && !isWalkable(n.x, n.y - dy))) {
                return n;
            }
            // n水平方向找跳点
            if (isWalkable(n.x + dx, n.y)) {
                if (jump(new PathNode(n.x + dx, n.y), n, end) != null) {
                    return n;
                }
            }
            // n垂直方向找跳点
            if (isWalkable(n.x, n.y + dy)) {
                if (jump(new PathNode(n.x, n.y + dy), n, end) != null) {
                    return n;
                }
            }
        } else if (dx != 0) {// 水平方向行进
            // dx=1时，下方有障碍物且右斜下可走 或者 上方有障碍物且右斜上可走  则n为跳点(n有强迫邻居右斜下或者右斜上)
            if ((isWalkable(n.x + dx, n.y + 1) && !isWalkable(n.x, n.y + 1))
                    || (isWalkable(n.x + dx, n.y - 1) && !isWalkable(n.x, n.y - 1))) {
                return n;
            }
        } else {// 垂直方向行进,dy!=0
            // dy=1时，右方有障碍物且右斜下可走 或者 左边有障碍物且左斜下可走 则n为跳点(n有强迫邻居右斜下或者左斜下)
            if ((isWalkable(n.x + 1, n.y + dy) && !isWalkable(n.x + 1, n.y) )
                    || (isWalkable(n.x - 1, n.y + dy) && !isWalkable(n.x - 1, n.y))) {
                return n;
            }
        }
        // 行进方向遇到障碍物或者超出边界 终止寻找跳点
        if (!isWalkable(n.x + dx, n.y + dy)) {
            return null;
        }
        //递归寻找跳点
        return jump(new PathNode(n.x + dx, n.y + dy), n, end);
    }

    private boolean isWalkable(int x, int y) {
        return isInBounds(x, y) && !isBlock(x, y);
    }

    private boolean isBlock(int x, int y) {
        return grid[x][y] == 1;
    }

    private boolean isInBounds(int x, int y) {
        return x >= 0 && x < grid.length && y >= 0 && y < grid[0].length;
    }

    private List<PathNode> reconstructPath(PathNode node) {
        LinkedList<PathNode> paths = new LinkedList<>();
        while (node != null) {
            paths.addFirst(node);
            node = node.getParent();
        }
        return paths;
    }

    /**
     * 预估函数
     *
     * @param a
     * @param b
     * @return
     */
    private int h(PathNode a, PathNode b) {
        //方形网格的曼哈顿距离
        return (Math.abs(a.getX() - b.getX()) + Math.abs(a.getY() - b.getY())) * 10;
    }

    /**
     * 这里会根据方向裁剪没必要的邻居
     * @param node
     */
    private void findNeighbors(PathNode node) {
        neighbors.clear();
        //这里也可以直接返回八个方向的邻居，下面的代码都是为了性能 裁剪没必要的邻居
//        if (true) {
//            fillAllNeighbors(node);
//            return;
//        }
        // 没有前节点的时候，直接返回八方向
        PathNode parent = node.parent;
        if (parent == null) {
            fillAllNeighbors(node);
            return;
        }
        //计算出行进方向
        int dx = getDx(node, parent);
        int dy = getDy(node, parent);
        // 斜线搜索
        if (dx != 0 && dy != 0) {
            // 下方或者上方可达，也就是垂直方向
            if (reachable(node, node.x, node.y + dy)) {
                neighbors.add(new PathNode(node.x, node.y + dy));
            }
            // 右边或者左边可达，也就是斜线的水平方向
            if (reachable(node, node.x + dx, node.y)) {
                neighbors.add(new PathNode(node.x + dx, node.y));
            }
            // 右下方或者左上方，也就是斜线同方向
            if (reachable(node, node.x + dx, node.y + dy)) {
                neighbors.add(new PathNode(node.x + dx, node.y + dy));
            }
            // dx=1时左边障碍物，dx=-1时右边障碍物
            if (!reachable(node, node.x - dx, node.y)) {
                // dy=1左斜下可达 ,,,,, dy=-1左斜上可达
                if (reachable(node, node.x - dx, node.y + dy)) {
                    neighbors.add(new PathNode(node.x - dx, node.y + dy));
                }
            }
            // dy=1上边有障碍物 dy=-1时下面有障碍物
            if (!reachable(node, node.x, node.y - dy)) {
                // dx=1时右斜上可达 ,,,,dx=-1时左斜上可达
                if (reachable(node, node.x + dx, node.y - dy)) {
                    neighbors.add(new PathNode(node.x + dx, node.y - dy));
                }
            }
        } else if (dx == 0) {//垂直方向
            // 下或者上可走，取决于dy=啥
            if (reachable(node, node.x, node.y + dy)) {
                neighbors.add(new PathNode(node.x, node.y + dy));
            }
            // 右边阻挡
            if (!reachable(node, node.x + 1, node.y)) {
                // dy=1时右斜下可达 , dy=-1时右斜上可达
                if (reachable(node, node.x + 1, node.y + dy)) {
                    neighbors.add(new PathNode(node.x + 1, node.y + dy));
                }
            }
            // 左边阻挡
            if (!reachable(node, node.x - 1, node.y)) {
                // dy=1左斜下可达 ,,,,, dy=-1左斜上可达
                if (reachable(node, node.x - 1, node.y + dy)) {
                    neighbors.add(new PathNode(node.x - 1, node.y + dy));
                }
            }
        } else { //水平方向
            // 右可达 左可达
            if (reachable(node, node.x + dx, node.y)) {
                neighbors.add(new PathNode(node.x + dx, node.y));
            }
            //下面阻挡
            if (!reachable(node, node.x, node.y + 1)) {
                // dx=1时右斜下可达 ,,,,dx=-1时左斜下可达
                if (reachable(node, node.x + dx, node.y + 1)) {
                    neighbors.add(new PathNode(node.x + dx, node.y + 1));
                }
            }
            // 上面阻挡
            if (!reachable(node, node.x, node.y - 1)) {
                // dx=1时右斜上可达 ,,,,dx=-1时左斜上可达
                if (reachable(node, node.x + dx, node.y - 1)) {
                    neighbors.add(new PathNode(node.x + dx, node.y - 1));
                }
            }
        }
    }

    private static int getDy(PathNode cur, PathNode pre) {
        return cur.y > pre.y ? 1 : (cur.y < pre.y ? -1 : 0);
    }

    private static int getDx(PathNode cur, PathNode pre) {
        return cur.x > pre.x ? 1 : (cur.x < pre.x ? -1 : 0);
    }

    /**
     * 没有Parent的时候 填充八方向的邻居，需要过滤不可走点
     *
     * @param n
     */
    private void fillAllNeighbors(PathNode n) {
        for (int i = -1; i <= 1; i++) {
            int y = n.getY() + i;
            for (int j = -1; j <= 1; j++) {
                int x = n.getX() + j;
                if (x < 0 || y < 0 || x >= grid[0].length || y >= grid.length) {
                    continue;
                }
                if (x == n.getX() && y == n.getY()) {
                    continue;
                }
                if (isBlock(x, y)) {
                    continue;
                }
                PathNode r = new PathNode(x, y);
                neighbors.add(r);
            }
        }
    }

    /**
     * from->to(x,y)是否可达,这里主要是判断特殊情况 斜线是否穿墙
     * 图示斜线穿墙
     * ------------
     * | 障碍 | TO |
     * ------------
     * |FROM| 障碍 |
     * ------------
     *
     * @param from
     * @param x
     * @param y
     * @return
     */
    protected boolean reachable(PathNode from, int x, int y) {
        if (!isWalkable(x, y)) {
            return false;
        }
        // 只要是水平方向 或者 垂直方向 且是可走的 就是可達的
        if (from.x == x || from.y == y) {
            return true;
        }
        // 避免斜线穿墙
        if (!isWalkable(x, from.y) && !isWalkable(from.x, y)) {
            return false;
        }
        return true;
    }

//    public static void main(String[] args) {
//        int[][] mapData = new int[12][12];
//        for (int i = 0; i < mapData.length; i++) {
//            for (int j = 0; j < mapData[0].length; j++) {
//                mapData[i][j] = 0;
//            }
//        }
//        MapFrame mapFrame = new MapFrame(mapData);
//        mapFrame.addFindPathConsumer(new Consumer<MapFrame>() {
//            @Override
//            public void accept(MapFrame mapFrame) {
//                JPS jps = new JPS();
//                List<PathNode> paths = jps.findPath(mapData, mapFrame.getStart(), mapFrame.getEnd());
//                List<int[]> path = paths.stream().map(e -> new int[]{e.getX(), e.getY()}).collect(Collectors.toList());
//                mapFrame.setPath(path);
//            }
//        });
//    }

}
